#include "pixel2mech.h"

PrOffset::PrOffset()
{
    X = 0;
    Y = 0;
    O_X = 0;
    O_Y = 0;
    W = 0;
    H = 0;
    Theta = 0;
}

PrOffset::PrOffset(double x, double y, double theta)
{
    X = x;
    Y = y;
    Theta = theta;
}

void PrOffset::ReSet()
{
    X = 0;
    Y = 0;
    Theta = 0;
    O_X = 0;
    O_Y = 0;
}

QString PrOffset::toString() const
{
    return QString("x: %1, y: %2, theta: %3, o_x: %4, o_y: %5").arg(X).arg(Y).arg(Theta).arg(O_X).arg(O_Y);
}

QString PrOffset::toShortString() const
{
    return QString("x: %1, y: %2, theta: %3").arg(X).arg(Y).arg(Theta);
}

PrOffset operator-(const PrOffset &left, const PrOffset &right)
{
    PrOffset prOffset;
    prOffset.X = left.X - right.X;
    prOffset.Y = left.Y - right.Y;
    prOffset.Theta = left.Theta - right.Theta;
    prOffset.O_X = left.O_X - right.O_X;
    prOffset.O_Y = left.O_Y - right.O_Y;
    return prOffset;
}

PrOffset operator*(const PrOffset &offset, double multiplier)
{
    PrOffset newOffset;
    newOffset.X = offset.X * multiplier;
    newOffset.Y = offset.Y * multiplier;
    newOffset.W = offset.W * multiplier;
    newOffset.H = offset.H * multiplier;
    newOffset.O_X = offset.O_X * multiplier;
    newOffset.O_Y = offset.O_Y * multiplier;
    return newOffset;
}

Pixel2Mech::Pixel2Mech()
{
    transformation = QMatrix();
    center_of_pixel = center_of_mech = QPointF(0.0, 0.0);
    has_calibration = false;
}

Pixel2Mech::Pixel2Mech(const QMatrix &matrix, const QPointF &center_of_pixel)
{
    ChangeParameter(matrix, center_of_pixel);
}

void Pixel2Mech::ChangeParameter(const QMatrix &matrix, const QPointF &center_of_pixel)
{
    transformation = matrix;
    this->center_of_pixel = center_of_pixel;
    center_of_mech = transformation.map(center_of_pixel);
    has_calibration = true;
}
QMatrix
Pixel2Mech::DoCalibration(const QPointF pixel_points[], const QPointF mech_points[], const QPointF &center_of_pixel)
{
    CMatrix M;
    M = getAffineTransform(pixel_points, mech_points);
    double *p_M_data = M.GetMatrixData();
    transformation.setMatrix(p_M_data[0], p_M_data[3], p_M_data[1], p_M_data[4], p_M_data[2], p_M_data[5]);
    this->center_of_pixel = center_of_pixel;
    center_of_mech = transformation.map(center_of_pixel);
    has_calibration = true;
    return transformation;
}

bool Pixel2Mech::CalcMechDistance(const QPointF &pixel_point, QPointF &mech_point)
{
    if (!has_calibration)
    {
        qCritical("Missing calibration");
        return false;
    }
    QPointF mech = transformation.map(pixel_point);
    QPointF mech_distence = mech - center_of_mech;
    mech_point.setX(mech_distence.x());
    mech_point.setY(mech_distence.y());
    return true;
}

QPointF Pixel2Mech::pixel2MechPoint(const QPointF &pixelPoint)
{
    if (has_calibration != true)
        return QPointF(0.0, 0.0);
    return transformation.map(pixelPoint);
}

QPointF Pixel2Mech::getMechCenter() const
{
    if (has_calibration != true)
        return QPointF(0.0, 0.0);
    return center_of_mech;
}

bool Pixel2Mech::hasCalibration()
{
    return has_calibration;
}

void Pixel2Mech::setHasCalibration(bool value)
{
    has_calibration = value;
}

double Pixel2Mech::getXResolution()
{
    return fabs(transformation.m12());
}

double Pixel2Mech::getYResoultion()
{
    return fabs(transformation.m21());
}

/* Calculates coefficients of affine transformation
 * which maps (xi,yi) to (ui,vi), (i=1,2,3):
 *
 * ui = c00*xi + c01*yi + c02
 *
 * vi = c10*xi + c11*yi + c12
 *
 * Coefficients are calculated by solving linear system:
 *         A     *     X  = B
 * / x0 y0 1 0 0 0 \ /c00\ /u0\
 * | 0 0 0 x0 y0 1 | |c01| |v0|
 * | x1 y1 1 0 0 0 | |c02| |u1|
 * | 0 0 0 x1 y1 1 | |c10| |v1|
 * | x2 y2 1 0 0 0 | |c11| |u2|
 * \ 0 0 0 x2 y2 1 / |c12| |v2|
 *
 * where:
 * cij - matrix coefficients
 */
CMatrix getAffineTransform(const QPointF src[], const QPointF dst[])
{

    double a[6 * 6] = { 0.0 }, b[6];
    CMatrix M;
    // fill the array
    for (int i = 0; i < 3; i++)
    {
        a[i * 2] = a[i * 2 + 19] = src[i].x();
        a[i * 2 + 6] = a[i * 2 + 19 + 6] = src[i].y();
        a[i * 2 + 6 + 6] = a[i * 2 + 19 + 6 + 6] = 1;

        b[i * 2] = dst[i].x();
        b[i * 2 + 1] = dst[i].y();
    }
    // make matrix by array
    CMatrix A(6, 6, a);
    CMatrix B(6, 1, b);
    // calc the answer:AX=B than X = A\B
    M = B.LeftDiv(A);
    return M;
}
